#include <ros/ros.h>
#include <base_controller/base_controller_stm32.h>

namespace base_controller{
    
    SerialParser::SerialParser(string port, int baud):serial_port_(port),serial_baud_(baud),serial_timeout_(2000){
        cout << "[Serial Params Set] serial_port: " << serial_port_ << " serial_baud: " << serial_baud_ << endl;

        // 打开串口
        try{ 
            serial_.setPort(serial_port_); 
            serial_.setBaudrate(serial_baud_); 
            // serial_.setFlowcontrol(serial::flowcontrol_none);
            // serial_.setParity(serial::parity_none); //default is parity_none
            // serial_.setStopbits(serial::stopbits_one);
            // serial_.setBytesize(serial::eightbits);
            serial::Timeout time_out = serial::Timeout::simpleTimeout(serial_timeout_);
            serial_.setTimeout(time_out);
            serial_.open(); 
        }
        catch (serial::IOException& e){
            ROS_ERROR_STREAM("[Serial Error] Unable to open port, " << e.what());
        }
        if(serial_.isOpen()){
            cout << "[Serial Connect] Serial Port initialized! Connected to Hardware on port " << serial_.getPort()  << " baud: " << serial_.getBaudrate() << endl;
        }

        //清空数据
        memset(&Robot_Vel, 0, sizeof(Robot_Vel));
        memset(&Receive_Data, 0, sizeof(Receive_Data)); 
        memset(&Send_Data, 0, sizeof(Send_Data));
        memset(&Imu_Data, 0, sizeof(Imu_Data));
    }

    SerialParser::~SerialParser(){
        SerialClose();
    }

    //关闭串口
    void SerialParser::SerialClose(){
        if (serial_.isOpen()){
            serial_.close();
            cout << "[Serial Connect] Serial Port closed successfully!" << endl;
        }
    }

    /**************************************
    功能: 串口通讯校验函数，数据包n有个字节，第n-1个字节为校验位，第n个字节位帧尾。第1个字节到第n-2个字节数据按位异或的结果与第n-1个字节对比，即为BCC校验
    输入参数： Count_Number：数据包前几个字节加入校验   mode：对发送数据还是接收数据进行校验
    ***************************************/
    unsigned char SerialParser::Check_Sum(unsigned char Count_Number,unsigned char mode){
        unsigned char check_sum=0,k;
        
        if(mode==0){//接收数据模式
            for(k=0;k<Count_Number;k++){
                check_sum=check_sum^Receive_Data.rx[k]; //By bit or by bit //按位异或
            }
        }
        if(mode==1){ //发送数据模式
            for(k=0;k<Count_Number;k++){
                check_sum=check_sum^Send_Data.tx[k]; //By bit or by bit //按位异或
            }
        }
        return check_sum; //Returns the bitwise XOR result //返回按位异或结果
    }

    short SerialParser::IMU_Trans(uint8_t Data_High,uint8_t Data_Low)
    {
        short transition_16;
        transition_16 = 0;
        transition_16 |=  Data_High<<8;   
        transition_16 |=  Data_Low;
        return transition_16;     
    }

    float SerialParser::Odom_Trans(uint8_t Data_High,uint8_t Data_Low)
    {
        float data_return;
        short transition_16;
        transition_16 = 0;
        transition_16 |=  Data_High<<8;  //Get the high 8 bits of data   //获取数据的高8位
        transition_16 |=  Data_Low;      //Get the lowest 8 bits of data //获取数据的低8位
        data_return   =  (transition_16 / 1000)+(transition_16 % 1000)*0.001; // The speed unit is changed from mm/s to m/s //速度单位从mm/s转换为m/s
        return data_return;
    }

    bool SerialParser::GetSensorData(){
        short transition_16=0;
        // ros::Time start_time = ros::Time::now();
        uint8_t check=0,Receive_Data_Pr[1];    //临时变量，保存下位机数据
        serial_.read(Receive_Data_Pr,sizeof(Receive_Data_Pr));
        if(Receive_Data_Pr[0] == FRAME_HEADER && !start_record){
            start_record = true;
            rec_data.push_back(Receive_Data_Pr[0]);
        }
        else if(start_record && rec_data.size() < RECEIVE_DATA_SIZE){
                rec_data.push_back(Receive_Data_Pr[0]);
        }
        if(rec_data.size() != RECEIVE_DATA_SIZE) return false;
        for(int n=0; n < RECEIVE_DATA_SIZE; n++){
            Receive_Data.rx[n] = rec_data[n];
        }
        vector<uint8_t> rec_data_swap;
        rec_data.swap(rec_data_swap);
        start_record = false;

        // ros::Time end_time = ros::Time::now();
        // double total_time = (end_time - start_time).toSec(); 
        // ROS_INFO("GetSensorData total_time: %0.5f",total_time);
        // ROS_INFO("0X%02X-0X%02X-0X%02X-0X%02X-0X%02X-0X%02X-0X%02X-0X%02X-0X%02X-0X%02X-0X%02X-0X%02X-0X%02X-0X%02X-0X%02X-0X%02X-0X%02X-0X%02X-0X%02X-0X%02X-0X%02X-0X%02X-0X%02X-0X%02X",
        //     Receive_Data.rx[0],Receive_Data.rx[1],Receive_Data.rx[2],Receive_Data.rx[3],Receive_Data.rx[4],Receive_Data.rx[5],Receive_Data.rx[6],Receive_Data.rx[7],
        //     Receive_Data.rx[8],Receive_Data.rx[9],Receive_Data.rx[10],Receive_Data.rx[11],Receive_Data.rx[12],Receive_Data.rx[13],Receive_Data.rx[14],Receive_Data.rx[15],
        //     Receive_Data.rx[16],Receive_Data.rx[17],Receive_Data.rx[18],Receive_Data.rx[19],Receive_Data.rx[20],Receive_Data.rx[21],Receive_Data.rx[22],Receive_Data.rx[23]);

        if(Receive_Data.rx[0] != FRAME_HEADER || Receive_Data.rx[23] != FRAME_TAIL) return false;  //帧头匹配0X7B 帧尾匹配0X7D
        if(Receive_Data.rx[22] != Check_Sum(22,READ_DATA_CHECK))  return false;     //BBC校验

        // ROS_INFO("0X%02X-0X%02X-0X%02X-0X%02X-0X%02X-0X%02X-0X%02X-0X%02X-0X%02X-0X%02X-0X%02X-0X%02X-0X%02X-0X%02X-0X%02X-0X%02X-0X%02X-0X%02X-0X%02X-0X%02X-0X%02X-0X%02X-0X%02X-0X%02X",
        //     Receive_Data.rx[0],Receive_Data.rx[1],Receive_Data.rx[2],Receive_Data.rx[3],Receive_Data.rx[4],Receive_Data.rx[5],Receive_Data.rx[6],Receive_Data.rx[7],
        //     Receive_Data.rx[8],Receive_Data.rx[9],Receive_Data.rx[10],Receive_Data.rx[11],Receive_Data.rx[12],Receive_Data.rx[13],Receive_Data.rx[14],Receive_Data.rx[15],
        //     Receive_Data.rx[16],Receive_Data.rx[17],Receive_Data.rx[18],Receive_Data.rx[19],Receive_Data.rx[20],Receive_Data.rx[21],Receive_Data.rx[22],Receive_Data.rx[23]);  

        Receive_Data.Frame_Header = Receive_Data.rx[0]; 
        Receive_Data.Frame_Tail = Receive_Data.rx[23]; 
        Receive_Data.Flag_Stop = Receive_Data.rx[1]; //set aside //预留位
        Robot_Vel.X = Odom_Trans(Receive_Data.rx[2],Receive_Data.rx[3]); //获取运动底盘X方向速度
        Robot_Vel.Y = Odom_Trans(Receive_Data.rx[4],Receive_Data.rx[5]); //获取运动底盘Y方向速度，Y速度仅在全向移动机器人底盘有效
        Robot_Vel.Z = Odom_Trans(Receive_Data.rx[6],Receive_Data.rx[7]); //获取运动底盘Z方向速度   
        Imu_Data.accele_x_data = IMU_Trans(Receive_Data.rx[8],Receive_Data.rx[9]);   //获取IMU的X轴加速度  
        Imu_Data.accele_y_data = IMU_Trans(Receive_Data.rx[10],Receive_Data.rx[11]); //Get the Y-axis acceleration of the IMU     //获取IMU的Y轴加速度
        Imu_Data.accele_z_data = IMU_Trans(Receive_Data.rx[12],Receive_Data.rx[13]); //获取IMU的Z轴加速度
        Imu_Data.gyros_x_data = IMU_Trans(Receive_Data.rx[14],Receive_Data.rx[15]);  //获取IMU的X轴角速度  
        Imu_Data.gyros_y_data = IMU_Trans(Receive_Data.rx[16],Receive_Data.rx[17]);  //获取IMU的Y轴角速度  
        Imu_Data.gyros_z_data = IMU_Trans(Receive_Data.rx[18],Receive_Data.rx[19]);  //获取IMU的Z轴角速度  
        //获取电池电压
        transition_16 |=  Receive_Data.rx[20]<<8;
        transition_16 |=  Receive_Data.rx[21];  
        Power_voltage = transition_16/1000+(transition_16 % 1000)*0.001; //Unit conversion millivolt(mv)->volt(v) //单位转换毫伏(mv)->伏(v)
        //cout << "Robot_Vel x: " << Robot_Vel.X << " y: " << Robot_Vel.Y << " z: " << Robot_Vel.Z << " Power_voltage: " << Power_voltage << endl;
        // end_time = ros::Time::now();
        // total_time = (end_time - start_time).toSec(); 
        // ROS_INFO("total_time: %0.3f",total_time);
        return true; 
    }


void SerialParser::SetMotorSpeed(double x, double y, double z){
    short  transition;  //中间变量
    Send_Data.tx[0]=FRAME_HEADER; //帧头0X7B
    Send_Data.tx[1] = 0; //预留位
    Send_Data.tx[2] = 0; //预留位

    //机器人x轴的目标线速度
    transition=0;
    transition = x*1000; //将浮点数放大一千倍，简化传输
    Send_Data.tx[4] = transition;     //取数据的低8位
    Send_Data.tx[3] = transition>>8;  //取数据的高8位
    //机器人y轴的目标线速度
    transition=0;
    transition = y*1000;
    Send_Data.tx[6] = transition;
    Send_Data.tx[5] = transition>>8;
    //机器人z轴的目标角速度
    transition=0;
    transition = z*1000;
    Send_Data.tx[8] = transition;
    Send_Data.tx[7] = transition>>8;

    Send_Data.tx[9]=Check_Sum(9,SEND_DATA_CHECK); 
    Send_Data.tx[10]=FRAME_TAIL;    //帧尾0X7D
    try{
        serial_.write(Send_Data.tx,sizeof (Send_Data.tx)); 
    }
    catch (serial::IOException& e)   {
        ROS_ERROR_STREAM("Unable to send data through serial port"); 
    }
}

}